﻿module Physics.Chapter3.RigidBody

open Casanova
open Casanova.Input
open Casanova.Core
open Casanova.Utilities
open Casanova.Math
open Casanova.Game
open Casanova.Drawing
open Casanova.StandardLibrary
open Casanova.StandardLibrary.Core
open Microsoft.Xna.Framework.Graphics
open Physics.Chapter3.Geometry

let mutable private max_index = 0

type [<CasanovaEntity>] RigidBody = {
  Model               : Casanova.Drawing.Model

  MassInv             : float32<kg^-1>
  IInv                : Matrix

  Position            : Rule<Vector3<m>>
  LinearMomentum      : Rule<Vector3<kg * m / s>>
  Rotation            : Rule<Quaternion>
  AngularMomentum     : Rule<Vector3<kg * m^2 / s>>

  F                   : Rule<Vector3<kg * m / s^2>>
  T                   : Rule<Vector3<kg * m^2 / s^2>>

  TickCount           : Rule<int>

  Vertices            : List<Vertex>
  Edges               : List<Edge>
  Faces               : List<Face>
  UniqueEdges         : List<Vector3<1>>

  mutable Index               : int
} with
  static member ModelPosition'(self : RigidBody) = !self.Position
  static member ModelRotation'(self : RigidBody) = !self.Rotation

  member body.AddF(F : Vector3<N>) =
    body.F := !body.F + F
  member body.AddT(T : Vector3<N * m>) =
    body.T := !body.T + T

  member self.Velocity = !self.LinearMomentum * self.MassInv
  static member Position'(self : RigidBody, dt : float32<s>) =
    !self.Position + self.Velocity * dt
  static member LinearMomentum'(self : RigidBody, dt : float32<s>) =
    !self.LinearMomentum + !self.F * dt
  static member F'(self : RigidBody, dt : float32<s>) =
    Vector3.Zero

  member self.R = Matrix.CreateFromQuaternion(!self.Rotation)
  member self.RInverse = Matrix.Transpose(self.R)
  member self.InverseI = 
    let R = Matrix.CreateFromQuaternion(!self.Rotation)
    R * self.IInv * Matrix.Transpose(R)
  member self.AngularVelocity = Microsoft.Xna.Framework.Vector3.Transform((!self.AngularMomentum).ToXNA, self.InverseI)
  member self.RotationVelocity = 
    let w = Microsoft.Xna.Framework.Quaternion(self.AngularVelocity, 0.0f)
    (w * !self.Rotation) * 0.5f
  static member Rotation'(self : RigidBody, dt : float32<s>) =
    let r' = !self.Rotation + self.RotationVelocity * (float32 dt)
    if !self.TickCount % 10 = 0 then
      Microsoft.Xna.Framework.Quaternion.Normalize(r')
    else
      r'
  static member AngularMomentum'(self : RigidBody, dt : float32<s>) =
    !self.AngularMomentum + !self.T * dt
  static member T'(self : RigidBody, dt : float32<s>) =
    Vector3.Zero

  member body.Immovable = body.MassInv = 0.0f<kg^-1>

  member body.FromLocalSpace(p:Vertex) = body.R * p.Position + !body.Position
  member body.FromLocalSpace(p:Vector3<m>) = body.R * p + !body.Position
  member body.FromLocalSpaceNormal(n:Vector3<'a>) = body.R * n
  member body.ToLocalSpace(p:Vector3<m>) = 
    Microsoft.Xna.Framework.Matrix.Transpose(body.R) * (p - !body.Position)
  member body.ToLocalSpaceNormal(p:Vector3<'a>) = 
    Microsoft.Xna.Framework.Matrix.Transpose(body.R) * p

  static member CreateCube(model_layer, position, size : Vector3<_>, mass_inv, friction_coefficient, path) =
    let cube_model  = Model.Create(model_layer, position, size * 0.5f, Microsoft.Xna.Framework.Quaternion.Identity, path)
    let mutable cube_IInv = Matrix.Identity
    cube_IInv.M11 <- float32(12.0f * mass_inv / (size.Y * size.Y + size.Z * size.Z))
    cube_IInv.M22 <- float32(12.0f * mass_inv / (size.X * size.X + size.Z * size.Z))
    cube_IInv.M33 <- float32(12.0f * mass_inv / (size.X * size.X + size.Y * size.Y))

    let vertices, faces, edges = CreateCube(size * 0.5f)
    let unique_edges = ResizeArray()
    for e in edges do
      let D = e.D
      if unique_edges |> Seq.exists (fun D' -> Vector3<1>.Dot(D',D) >= 0.99f) |> not then
        unique_edges.Add D

    do max_index <- max_index + 1

    {
      Model               = cube_model

      MassInv             = mass_inv
      IInv                = cube_IInv

      Position            = Rule.Create(position)
      LinearMomentum      = Rule.Create(Vector3.Zero)
      Rotation            = Rule.Create(Quaternion.Identity)
      AngularMomentum     = Rule.Create(Vector3.Zero)

      F                   = Rule.Create(Vector3.Zero)
      T                   = Rule.Create(Vector3.Zero)

      TickCount           = Rule.Create(0)
  
      Vertices            = vertices
      Faces               = faces
      Edges               = edges
      UniqueEdges         = unique_edges |> Seq.toList

      Index               = max_index - 1
    }

  member self.ApplyExternalForce(r : Vector3<m>, f_ext : Vector3<N>) =
    // we must transform the point of application from world space to local space;
    let r     = self.ToLocalSpace(r).ToXNA
    let f_ext = f_ext.ToXNA

    let F = r * Microsoft.Xna.Framework.Vector3.Dot(f_ext, r)
    let T = Microsoft.Xna.Framework.Vector3.Cross(r, f_ext)

    self.F := !self.F + Vector3<_>(F)
    self.T := !self.T + Vector3<_>(T)

  static member private ContinuousAxisTest(body1 : RigidBody, body2 : RigidBody, N : Vector3<1>, min2, max2, vrel : Vector3<m/s>, dt : float32<s>, collision_out : CollisionType<RigidBody>) =
    let collision = collision_out,min2,max2,0.0f<s>
    if min2 > -0.01f<m> then // separating now; check if closing in
      let s = Vector3<_>.Dot(vrel, N)
      if s >= 0.0f<m/s> then // separating further; this is a separating axis
        collision
      else // look for a time t such that t < dt and min - s * t = 0
        let t = min2 / s
        if t < dt then // does not close-in in time; this is a separating axis
          collision
        else // closes in at time t; this is not a separating axis
          collision_out,0.0f<m>,max2,t
    else
      collision


  static member private FaceSAT(body1 : RigidBody, body2 : RigidBody, dt : float32<s>) =
    let find_closest(body : RigidBody, P : Vector3<m>, D : Vector3<1>) =
      let D' = body.ToLocalSpaceNormal D
      let offset = Vector3<1>.Dot(P, D)
      let v_max = body.Vertices |> Seq.maxBy (fun V -> Vector3<1>.Dot(V.Position, D'))
      let max = Vector3<1>.Dot(body.FromLocalSpace(v_max.Position), D) - offset
      if max < -0.1f<m> then
        None
      else
        let v_min = body.Vertices |> Seq.minBy (fun V -> Vector3<1>.Dot(V.Position, D'))
        let min = Vector3<1>.Dot(body.FromLocalSpace(v_min.Position), D) - offset
        Some(v_min,v_max,min,max)
    let Vrel = (body1.Velocity - body2.Velocity) * 1.0f
    seq{
      for face1 in body1.Faces do
        // reference vertex in world space
        let P = body1.FromLocalSpace(face1.Center)
        // separating direction in world space
        let N = body1.FromLocalSpaceNormal face1.Normal
        match find_closest(body2, P, N) with
        | Some(v_min2,v_max2,min2,max2) -> 
          yield RigidBody.ContinuousAxisTest(body1, body2, N, min2, max2, Vrel, dt, CollisionType.FaceFace(body1, body2, face1, face1, v_min2, [], []))
        | _ -> ()
    }


  static member private EdgeSAT(body1 : RigidBody, body2 : RigidBody, dt : float32<s>) =
    let find_interval(body : RigidBody, D : Vector3<1>) =
      let D' = body.ToLocalSpaceNormal D
      let v_min = body.Vertices |> Seq.minBy (fun V -> Vector3<1>.Dot(V.Position, D'))
      let v_max = body.Vertices |> Seq.maxBy (fun V -> Vector3<1>.Dot(V.Position, D'))
      Vector3<1>.Dot(body.FromLocalSpace(v_min), D), Vector3<1>.Dot(body.FromLocalSpace(v_max), D)
    let Vrel = (body1.Velocity - body2.Velocity) * 1.0f
    seq{
      for e1 in body1.UniqueEdges do
        for e2 in body2.UniqueEdges do
          // separating direction in world space
          let D1, D2 = body1.FromLocalSpaceNormal e1, body2.FromLocalSpaceNormal e2
          if abs(Vector3<1>.Dot(D1, D2)) < 0.8f then
            let N = Vector3<1>.Cross(D1, D2)
            if N.Length >= 0.9f then
              let min1,max1 = find_interval(body1, N)
              let min2,max2 = find_interval(body2, N)
              if max1 >= -0.01f<m> && max2 >= -0.01f<m> then
                yield RigidBody.ContinuousAxisTest(body1, body2, N, min1 - max2, System.Single.NegativeInfinity * 1.0f<m>, Vrel, dt, CollisionType.EdgeEdge(body1, body2, body1.Edges.Head, body2.Edges.Head, Vector3.Zero, Vector3.Zero))
                yield RigidBody.ContinuousAxisTest(body1, body2, N, min2 - max1, System.Single.NegativeInfinity * 1.0f<m>, Vrel, dt, CollisionType.EdgeEdge(body1, body2, body1.Edges.Head, body2.Edges.Head, Vector3.Zero, Vector3.Zero))
      }

  static member GetSatCollisions(body1 : RigidBody, body2 : RigidBody, dt : float32<s>) =
    let face_separations = 
      seq{
        yield! RigidBody.FaceSAT(body1, body2, dt)
        yield! RigidBody.FaceSAT(body2, body1, dt)
      }
    let edge_separations = RigidBody.EdgeSAT(body1, body2, dt)
    let separations =
      seq{
        yield! face_separations
        yield! edge_separations
      }
    match separations |> Seq.tryFind (fun (c,min_d,max_d,t) -> min_d > 0.01f<m>) with
    | Some _ -> NoCollision
    | None ->
      let least_colliding = face_separations |> Seq.maxBy (fun (c,min_d,max_d,t) -> min_d)

      let collision,min_distance,max_distance,collision_time = least_colliding
      match collision with
      | FaceFace(body1,body2,f1,f2,v2,vs1,vs2) ->
        let f1,f2,vs1,vs2 = FaceToFaceCollisions(f1, v2, body1.Immovable, body2.Immovable, collision_time, body1.Velocity, body2.Velocity, body1.Faces, body2.Faces, body1.FromLocalSpace, body2.FromLocalSpace, body1.FromLocalSpaceNormal, body2.FromLocalSpaceNormal)
        FaceFace(body1,body2,f1,f2,v2,vs1,vs2)
      | _ -> NoCollision // this should never happen at this point
